#include "contour_expand.h"
#include <cmath>
#include <list>
#include <cstdlib>

void swap2(int* one,int* two){
    int temp=*one;
    *one=*two;
    *two=temp;
}

Contour* contourexpand(Contour* contorno, Binary_Image* imgguide){

    list<Position> pointsofcontour=contorno->points;

    Contour* newcontour = new Contour(imgguide);

    //for all original contour points
    int distancewalk=0;
    int maxlenght=contorno->getPixelLenght();
    for(list<Position>::iterator pointtoexpand = pointsofcontour.begin();pointtoexpand!=pointsofcontour.end();pointtoexpand++){

        Position newcontourpoint=*pointtoexpand;

        list<Position>::iterator towalk=pointtoexpand;
        list<Position>::iterator next,back;

        Position position1,position2;

        //get tangent vector
        if(distancewalk+4>maxlenght){
            int diference=(distancewalk+4)-maxlenght;
            int nextindex,backindex;
            nextindex=3-diference;
            backindex=2+diference;
            next=towalk;
            back=towalk;
            for(int i=0;i<nextindex;i++)
                next++;
            for(int i=0;i<backindex;i++)
                back--;
            position1=*back;
            position2=*next;
        }
        else if(distancewalk<2){
            int diference=2-distancewalk;
            int nextindex,backindex;
            nextindex=3+diference;
            backindex=2-diference;
            next=towalk;
            back=towalk;
            for(int i=0;i<nextindex;i++)
                next++;
            for(int i=0;i<backindex;i++)
                back--;
            position1=*back;
            position2=*next;
        }
        else{
            int nextindex,backindex;
            nextindex=3;
            backindex=2;
            next=towalk;
            back=towalk;
            for(int i=0;i<nextindex;i++)
                next++;
            for(int i=0;i<backindex;i++)
                back--;
            position1=*back;
            position2=*next;
        }
        Vector tangent=pointfromposition(position2)-pointfromposition(position1);

        //the perpendicular vector
        Vector perp(tangent.y,-tangent.x);
        perp.normalize();

        //Get guide point to give one line equation  in correct direction.
        float quadrante = perp*Vector(1,0);
        float quadrante2 = perp*Vector(0,1);

        Position linepoint;
        int xguide,yguide;
        if(fabs(quadrante)>0.7){
            if(quadrante>0){
                xguide=imgguide->getWidth()-1;
                yguide=(perp.y/perp.x)*(xguide-(*pointtoexpand).x)+(*pointtoexpand).y;
                linepoint=Position(xguide,yguide);
            }
            else{
                xguide=0;
                yguide=(-perp.y/perp.x)*(*pointtoexpand).x+(*pointtoexpand).y;
                linepoint=Position(xguide,yguide);
            }
        }
        else{
            if(quadrante2<0){
                yguide=imgguide->getHeight()-1;
                xguide=(perp.x/perp.y)*(yguide-(*pointtoexpand).y)+(*pointtoexpand).x;
                linepoint=Position(xguide,yguide);
            }
            else{
                yguide=0;
                xguide=(perp.x/perp.y)*(yguide-(*pointtoexpand).y)+(*pointtoexpand).x;
                linepoint=Position(xguide,yguide);
            }
        }

        //Bresenham
        int x0,x1,y0,y1;
        x0=(*pointtoexpand).x;
        y0=(*pointtoexpand).y;
        x1=linepoint.x;
        y1=linepoint.y;


        int Dx = x1 - x0;
        int Dy = y1 - y0;
        int steep = (abs(Dy) >= abs(Dx));
        if (steep) {
            swap2(&x0, &y0);
            swap2(&x1, &y1);
       // recompute Dx, Dy after swap
            Dx = x1 - x0;
            Dy = y1 - y0;
        }
        int xstep = 1;
        if (Dx < 0) {
            xstep = -1;
            Dx = -Dx;
        }
        int ystep = 1;
        if (Dy < 0) {
            ystep = -1;
            Dy = -Dy;
        }
        int TwoDy = 2*Dy;
        int TwoDyTwoDx = TwoDy - 2*Dx; // 2*Dy - 2*Dx
        int E = TwoDy - Dx; //2*Dy - Dx
        int y = y0;
        int xDraw, yDraw;
        for (int x = x0; x != x1; x += xstep) {
            if (steep) {
                xDraw = y;
                yDraw = x;
            } else {
                xDraw = x;
                yDraw = y;
            }
            // plot
            if(!imgguide->getPixel(xDraw,yDraw)){
                newcontourpoint=Position(xDraw,yDraw);
                break;
            }
            // next
            if (E > 0) {
                E += TwoDyTwoDx; //E += 2*Dy - 2*Dx;
                y = y + ystep;
            } else {
                E += TwoDy; //E += 2*Dy;
            }
        }

        newcontour->addPoint(newcontourpoint);
        distancewalk++;
    }

    return newcontour;

}
